/*
  File:    DiscreteReinit.cpp
  Purpose: Fast but inaccurate level set reinitialization.   

  Contact: Paul Macklin
           pmacklin@math.uci.edu
		  http://math.uci.edu/~pmacklin
*/

#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <cmath>

using namespace std;

#include "../Matrix.h"
#include "../ImprovedMath.h"
#include "Contour.h"

#include "DiscreteReinit.h"

double MinimumDistanceSquared( Contour& InputContour , double x, double y )
{
 double BestDistance = 98e99;
 double SecondBestDistance = 99e99;
 
 DataPoint* pCurrentDataPoint = InputContour.pFirstDataPoint; 
  
 DataPoint* pBestDataPoint = NULL;
 DataPoint* pSecondBestDataPoint = NULL;
 
 while( pCurrentDataPoint )
 {
  double distance = square( pCurrentDataPoint->x - x ) + square( pCurrentDataPoint->y - y );
  if( distance < SecondBestDistance )
  {
   if( distance < BestDistance )
   {
    pSecondBestDataPoint = pBestDataPoint;
    SecondBestDistance = BestDistance;
    
    pBestDataPoint = pCurrentDataPoint;
    BestDistance = distance; 
   }
   else
   {
    pSecondBestDataPoint = pCurrentDataPoint;
	SecondBestDistance = distance;
   }
  }
  pCurrentDataPoint = pCurrentDataPoint->pNextDataPoint;
 }

 if( pBestDataPoint == pSecondBestDataPoint || !pSecondBestDataPoint )
 {
  return BestDistance;
 }
  
 double s = 0.0;
 int direction = 1;
 double ds =0.1;
 double tolerance = 1e-10;
 double Dist = 8e9;
 double PrevDist = 9e9;
 
 double dX = pSecondBestDataPoint->x - pBestDataPoint->x;
 double dY = pSecondBestDataPoint->y - pBestDataPoint->y;
 
 while( ds > tolerance )
 {
  while( Dist < PrevDist ) // && s <= 1.0 && s >= 0.0 )
  {
   PrevDist = Dist;
   double Xtemp = pBestDataPoint->x + s*dX;
   double Ytemp = pBestDataPoint->y + s*dY;
   Dist = square( x - Xtemp ) + square( y - Ytemp );
   s += ds*direction;
  }
  if( s > 1.0 ){ s = 1.0; }
  if( s < 0 ){ s = 0.0; }
  
  ds *= 0.1;
  direction *= -1;   
 }
 return Dist; 
 // return BestDistance;
}

Matrix DiscreteReinit(Matrix &mIn)
{
 extern double _EPS;
 extern int Xnodes;
 extern int Ynodes;
 extern double dx;
 extern double dy;
 
 DataPoint* pHeadDataPoint;
 pHeadDataPoint = new DataPoint;

 int i,j;

 Matrix Output = zeros( mIn.TellRows(), mIn.TellCols() );
 
 // find the contour points
 
 Contour ZeroContour;
 DataPoint* pCurrentDataPoint = ZeroContour.pFirstDataPoint;
 
 for( i=0 ; i < mIn.TellRows() ; i++ )
 {
  for( j=0 ; j < mIn.TellCols()-1 ; j++ )
  {
   bool FoundPoint = false;
   if( *mIn(i,j) <= _EPS & *mIn(i,j+1) > _EPS )
   { FoundPoint = true; }
   if( *mIn(i,j) > _EPS & *mIn(i,j+1) <= _EPS )
   { FoundPoint = true; }
   if( FoundPoint )
   {
    double x = i; 
    double y = j + fabs( *mIn(i,j) ) / ( fabs(*mIn(i,j)) + fabs(*mIn(i,j+1)) );	
    DataPoint* pNewDataPoint;
    pNewDataPoint = new DataPoint;
    pNewDataPoint->x = x;
	pNewDataPoint->y = y;
	if( pCurrentDataPoint )
	{ pCurrentDataPoint->pNextDataPoint = pNewDataPoint; }
	else
	{ ZeroContour.pFirstDataPoint = pNewDataPoint; }
    pCurrentDataPoint = pNewDataPoint;	    
   }
  
  }
 }
 
 for( j=0 ; j < mIn.TellCols() ; j++ )
 {
  for( i=0 ; i < mIn.TellRows()-1 ; i++ )
  {
   bool FoundPoint = false;
   if( *mIn(i,j) <= _EPS & *mIn(i+1,j) > _EPS )
   { FoundPoint = true; }
   if( *mIn(i,j) > _EPS & *mIn(i+1,j) <= _EPS )
   { FoundPoint = true; }
   if( FoundPoint )
   {
    double x = i+ fabs( *mIn(i,j) ) / ( fabs(*mIn(i,j)) + fabs(*mIn(i+1,j)) );
    double y = j;
    DataPoint* pNewDataPoint;
    pNewDataPoint = new DataPoint;
    pNewDataPoint->x = x;
	pNewDataPoint->y = y;
	if( pCurrentDataPoint )
	{ pCurrentDataPoint->pNextDataPoint = pNewDataPoint; }
	else
	{ ZeroContour.pFirstDataPoint = pNewDataPoint; }
    pCurrentDataPoint = pNewDataPoint;	    
   }
  
  }
 }

 // if no points were found, return an all-ones distance 
 // function.  
 if( !pCurrentDataPoint )
 {
  Output = ones(Xnodes,Ynodes); 
  return Output;
 }

 // compute (discrete) distance to the contour for each point
 
 for( j=0 ; j < mIn.TellCols() ; j++ )
 {
  for( i=0 ; i < mIn.TellRows() ; i++ )
  {
   double Sign = 1.0;
   if( *mIn(i,j) <= _EPS )
   { Sign = -1.0; }
   *Output(i,j) = dx*Sign*sqrt( MinimumDistanceSquared(ZeroContour,i,j) );
  }
 } 
 
 return Output;
} 

// class stuff

ReinitMatrix::~ReinitMatrix()
{
 int i=0;
 for(i=0;i<Rows;i++)
 {
  delete [] Nodes[i];
 }
 delete [] Nodes;
 cout << __FILE__ << " : " << __LINE__ << endl;
}

ReinitMatrix::ReinitMatrix()
{
 Rows = 1;
 Cols = 1;
 Nodes = new ReinitNode* [1];
 Nodes[0] = new ReinitNode [1];
}

void ReinitMatrix::SetSize(int NewRows, int NewCols)
{
 for( int i=0 ; i < Rows ; i++ )
 { delete [] Nodes[i]; }
 delete [] Nodes;
 
 Rows = NewRows;
 Cols = NewCols; 

 Nodes = new ReinitNode* [ Rows ];
 int i=0, j=0; 
 for(i=0; i<Rows; i++)
 {
  Nodes[i] = new ReinitNode [ Cols ];
 }
 
 for( i=0 ; i < Rows ; i++ )
 {
  for( j=0 ; j < Cols ; j++ )
  {
   (Nodes[i][j]).i = i;
   (Nodes[i][j]).j = j;
  }
 }
 
 
 return;
}

ReinitNode* ReinitMatrix::operator()(int i, int j)
{
 return &(Nodes[i][j]);
}

ReinitNode::ReinitNode()
{
 Trial = false;
 Known = false;
 Far   = false;
 Data  = 0.0;
 pNextReinitNode = NULL;
 i=0;
 j=0;
}

double UpdateNode( ReinitMatrix& Input , int i, int j )
{
 extern double _EPS;
 extern double dx;

 // decide which values to use for the gradient approximation;
 // be sure to respect matrix bounds 
 
 double ValueX;
 double ValueXleft, ValueXright;
 if( i > 0 )
 { ValueXleft = Input(i-1,j)->Data; }
 else
 { ValueXleft = 9e99; }
 if( i < Input.Rows-1 )
 { ValueXright = Input(i+1,j)->Data; }
 else
 { ValueXright = 9e99; }
 
 if( fabs( ValueXleft ) < fabs( ValueXright ) )
 { ValueX = ValueXleft; }
 else
 { ValueX = ValueXright; }
 
 double ValueY;
 double ValueYleft, ValueYright;
 if( j > 0 )
 { ValueYleft = Input(i,j-1)->Data; }
 else
 { ValueYleft = 9e99; }
 if( j < Input.Cols-1 )
 { ValueYright = Input(i,j+1)->Data; }
 else
 { ValueYright = 9e99; }
 
 if( fabs( ValueYleft ) < fabs( ValueYright ) )
 { ValueY = ValueYleft; }
 else
 { ValueY = ValueYright; }

 // construct the quadratic polynomial for the value 
 // we seek: Ax^2 + Bx + C, where x is the updated node;
 // This whole thing comes from solving 
 // square( (x-ValueX)/DeltaX ) + square( (x-ValueY)/DeltaX ) = 1.0 

 double A,B,C; 
 
 A = 2.0;
 B = -2.0*(ValueX+ValueY);
 C = square(ValueX) + square(ValueY) - square(dx);
 
 // find the two solutions of the quadratic polynomial 
 
 double Discriminant = square( B ) - 4*A*C;
 double Denominator = 4.0; // 2.0*A;

 if( Discriminant <= 0.0 )
 {
  return -B / Denominator;
 }
 Discriminant = sqrt( Discriminant );
 
 double Solution1 = (-B - Discriminant)/Denominator;
 double Solution2 = (-B + Discriminant)/Denominator;
 
 // choose a solution 
 
 double CurrentValue = Input(i,j)->Data;
 if( CurrentValue*Solution1 < _EPS )
 { return Solution2; }
 if( CurrentValue*Solution2 < _EPS )
 { return Solution1; }
 if( fabs( Solution1 ) < fabs( Solution2 ) )
 { return Solution2; }
 return Solution1; 
}

Matrix LinearDiscreteReinit( Matrix& Input )
{
 extern double _EPS;
 extern Matrix X;
 extern Matrix Y;
 
 DataPoint* pHeadDataPoint;
 pHeadDataPoint = new DataPoint;

 int i,j;

 Matrix Output = zeros( Input.TellRows(), Input.TellCols() );
 
 // find the contour points
 
 Contour ZeroContour;
 DataPoint* pCurrentDataPoint = ZeroContour.pFirstDataPoint;
 
 for( i=0 ; i < Input.TellRows() ; i++ )
 {
  for( j=0 ; j < Input.TellCols()-1 ; j++ )
  {
   bool FoundPoint = false;
   if( *Input(i,j) <= _EPS & *Input(i,j+1) > _EPS )
   { FoundPoint = true; }
   if( *Input(i,j) > _EPS & *Input(i,j+1) <= _EPS )
   { FoundPoint = true; }
   if( FoundPoint )
   {
    double x = i; 
    double y = j + fabs( *Input(i,j) ) / ( fabs(*Input(i,j)) + fabs(*Input(i,j+1)) );	
    DataPoint* pNewDataPoint;
    pNewDataPoint = new DataPoint;
    pNewDataPoint->x = x;
	pNewDataPoint->y = y;
	if( pCurrentDataPoint )
	{ pCurrentDataPoint->pNextDataPoint = pNewDataPoint; }
	else
	{ ZeroContour.pFirstDataPoint = pNewDataPoint; }
    pCurrentDataPoint = pNewDataPoint;	    
   }
  
  }
 }
 
 for( j=0 ; j < Input.TellCols() ; j++ )
 {
  for( i=0 ; i < Input.TellRows()-1 ; i++ )
  {
   bool FoundPoint = false;
   if( *Input(i,j) <= _EPS & *Input(i+1,j) > _EPS )
   { FoundPoint = true; }
   if( *Input(i,j) > _EPS & *Input(i+1,j) <= _EPS )
   { FoundPoint = true; }
   if( FoundPoint )
   {
    double x = i+ fabs( *Input(i,j) ) / ( fabs(*Input(i,j)) + fabs(*Input(i+1,j)) );
    double y = j;
    DataPoint* pNewDataPoint;
    pNewDataPoint = new DataPoint;
    pNewDataPoint->x = x;
	pNewDataPoint->y = y;
	if( pCurrentDataPoint )
	{ pCurrentDataPoint->pNextDataPoint = pNewDataPoint; }
	else
	{ ZeroContour.pFirstDataPoint = pNewDataPoint; }
    pCurrentDataPoint = pNewDataPoint;	    
   }
  
  }
 }

 // compute (discrete) distance to the contour for each point
 // use a linear approximation to the curve using the closest 
 // and second-closest points 
 
 for( j=0 ; j < Input.TellCols() ; j++ )
 {
  for( i=0 ; i < Input.TellRows() ; i++ )
  {
   double Sign = 1.0;
   if( *Input(i,j) <= _EPS )
   { Sign = -1.0; } 
   *Output(i,j) = Sign*sqrt( MinimumDistanceSquared(ZeroContour,*X(i),*Y(j)) );
  }
 } 

 return Output;
}

Matrix DiscreteReinit(Matrix &Input, Contour& ZeroContour )
{
 extern double _EPS;
 extern Matrix X;
 extern Matrix Y;

 Matrix Output = ones( Input.TellRows(), Input.TellCols() );
 
 if( ! ZeroContour.pFirstDataPoint )
 { return Output; }
 
 // compute (discrete) distance to the contour for each point
 // use a linear approximation to the curve using the closest 
 // and second-closest points 
 
 int i,j;
 for( j=0 ; j < Input.TellCols() ; j++ )
 {
  for( i=0 ; i < Input.TellRows() ; i++ )
  {
   double Sign = 1.0;
   if( *Input(i,j) <= _EPS )
   { Sign = -1.0; } 
   *Output(i,j) = Sign*sqrt( MinimumDistanceSquared(ZeroContour,*X(i),*Y(j)) );
  }
 }  
 
 return Output;
}
